#include <lwr_controllers/RPY.h>

int32 id
geometry_msgs/Vector3 position
RPY orientation
